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A8«sarr: A Simple strategy for the attitude control and arm coordination of a maneuverable 
space robot with dual arms is proposed. The basic task for the robot consists of the 
placement of marked rigid solid objects with specified pairs of gripping points and a speci¬ 
fied direction of approach for gripping. The strategy consists of three phases each of 
which involves only elementary rotational and translational collision-free nanouvecs of the 
robot body. Control laws for these elementary maneuvers are derived by using a body- 
referenced dynamic model of the dual-arm robot. 

1. INTRODUCTION 

la th. design or orbital m.n.uvsrih, vehicle, (GHV) with ““ 

stetion assembly and maintenance, and In contradistinction with earth-based tired robots, 

Tyst. C i: into consideration the interaction between tb. vehicle attited. ahd 

robot arm motions. / 

this paper. -« propose a simple strategy tor attitude control andarm «“‘> i "«^°;„*j l ^ W “^ r "t 1 on 

task is presented. The paper concludes with a brief description of the work m progress. 

2. ROBOT AND TASK DESCRIPTIONS / 

Figure 1 shows the basic configuration of the CMV duai-arm robot under consideration For^simplicity.the^ 

main frame of the CMV is represented by a maneuverable riqid body which provu es a 

assum^that^he arms have Ohly rotary joint, and .their motion, -1th respect to the base are planar. 

The basic task tor the * robot consists of placement o, marked rigid object. « .parked 

sider*the problel ofdeteLining the optimal gripping points and 

the end-effector size and the mode of gripping. igure . , h for both end-effectors is specified, 

planar dual-arm robot. ,n the first mode, a single drr.ction ,of W™* ~ ‘ of gripping 

in the second mod., the end-etteotors may approach the rod ln relative orient.- 

mode should depend on the placement objective and gripping .tablll y • • of ability to qrasp 

tion between the end-effectors and the object during the approach does not result in the loss 

the object). 

3. BASIC REQUIREMENT AND CONSTRAINTS 

Before discussing the nrohlcn of deriving suitable control strategies for the off robot to perfo™ the basic 
task, we first consider the basic requirements and physical constraints assoclatet a»i-.i e 


space » occup - »y don^in of the entire robot at time t is denoted by , R lt) = 

Ur'TtVJ? (tf,! We~assume~that T. (t) has a pair of specified gripping points ^(t) and jrlt) on % (t) 

Jch're time- invariant with respect to any fixed body-frame of 7.JU £ t§T = 

codVloVtt)}) (the convex hull of the set {■}) lies in a plane .Ii Q <tl with normal ^(t) correspond 

specified direct ion of approach for gripping by the end-effectors of both robot arms. 

Since the CMV is to be an autonomous or self-contained system, it is n-tural to introduce a coordinate 

Since tneUHV reference frame for the arm motions and the vision system. tor convenience, 

system C which serves as the basic reierence , te the time-dependent 

the origin of C B is fixed at the root of one of the arms. Let e* (t) ,e y t ,e^ 
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basis 6 fcr C . We align e (t) with the axis of the first rotary joint of arm 1 (see Fig.l). 

t B _ 2 

Let p‘(t> and p 2 (t> denote the base points 0 and 0* of links 1 and 1* at time t respectively. The line seg¬ 
ment I.Hft) « co({p>(t) ,p? (t) }) and normal r»{t> - e (t) define a plane n (t), where r. tt) specifies the heading 
of the R 0MV robot a? time t. The position R of the-'end-effector of arm i (may be taken as the tip of the second 
link of arm i) is denoted by £^(t), and the deviations £^(t) “ E^ (t) and % (t) “ by ^ (t) and re ~ 

spectively. 

Now, the basic requirements and constraints associated with the control of OMV robot can be stated as follows: 

(a) Before the end-effectors are in contact with the specified gripping points of the object, any OMV maneuvers 
and arm movements must be collision-free. This implies that 

(i) g} (t)0 3. v (t) => 0 (no robot-object collisions); 

R o 

(ii) r M or. (t)u . <t)> | n l U C3r T ,<t)U 31 ,, (t))l = 0 (no arm-arm collisions ); 

i-1,2 J1 Ll j-X',2* ^ ] 


(iii> 3:: (t)D3r (t) = 0 and 3V . (t) n 3Z (t) = 0, j=2,2' (no second link (joint)-base collisions) 

Li B Jl w 

at any time tElO,^, where corresponds to the first time when A£ X (t^ = 0, i-1,2 and Ant^) “ 2. {or II A D. <t: i 5 II 

+ ! I(t )tU a given positive number). The foregoing conditions may be relaxed by allowing point contacts 

with^zero velocity. In the case where a specified clearance between any two components of the robot must be 
maintained, we may enclose each member by a boundary layer with prescribed thickness, and impose conditions (i)- 

(iii) to the outside boundary of the layers. 

(b) Each end-effector should tend to its designated gripping point in a smooth non-oscillatory manner during 
its final approach. This can be fulfilled by requiring ||A£M*)|| and |JAr^-> || to be smooth strictly monotone de¬ 
creasing functions of t over some • -hinterval [t* ,t J C [0 ,t ], and || Ajs 1 (t ) || < C . , i-1,2 and || Ari (t j) || v. C , w ere C Q # 
f ; an a f- 2 are specified nonnegative numbers. To ensure acceptable relative velocities between the end-effectors 

and the gripping points of the object when they are within the gripping range, additional velocity constraints: 
||A£Mt)|j 4 tT. ,i»l,2 and || Ajh (t^jf may be introduced. 

(c) To achieve complete autonomy of the OMV robot, the control strategies or control laws should depend only on 
on-board sensor data. Moreover, they should be sufficiently simple so as to permit on-board real-time implemen¬ 
tation . 

Evidently, the incorporation of the foregoing requirements and constraints into the formulation of any control 
problem leads to formidable difficulties. A basic difficulty is that the characterization of the class of con¬ 
trols which generate the co1 Usion-free maneuvers and monotone approach is not readily obtainable. In what 
follows, wf* propose a simple approach which bypasses the abovementioned difficulty. 

4. PROPOSED STRATEGY 

The basic idea ll to decomoose the robot control problem into three phases; 

(PI) Alignment Phase: The objective is to maneuver the CMV so that its heading n R is aligned with the object's 
gripping direction n"‘. Moreover, the arms are prepositioned to achieve the required end-effector orientation 
and position so that°the object can be grasped by a subsequent straight-line translational motion of the OMV. 

Since it is difficult to achieve collision-free OMV maneuvers and arm movements when the object is close to the 
OMV robot, we propose to move the GMV sufficiently far away from the object before initiating any alignment maneu¬ 
ver and arm prepositioning. 

(p2) Acquisition Phase-. With the arms' joint angles locked in the preset values, the GMV moves along a ^ olll_ 
sion-free straight-line path to rendez-vous with the object in a monotone manner. The attitude end translation¬ 
al motion control systems at the OMV base maintain the deviations ,i An (t)|| , j| An(t )|| ,|| <-£»■ (t)|| ,|| A£ (t)!| , i-l,- within 
the acceptable values at all times during the rendez-vous* 

(PJ) Task Phase: After graspping the object by means of the end-effectors, the joint angles are again Locked. 
I’hen the GMV moves to the required destination and places the object there with the specified orientation. 

Finally, the UMV backs away from the object along a straight-line path. 

The hoice of .= traicht-1ine oaths for translational motions is motivated from tne tact that complex maneuvers 
of the CMV in space should be avoided,since such maneuvers could be catastrophic in case of control system failure. 
The proposed locking of all joints during any base attitude alignment and translational maneuvers a-. -- , the possi¬ 
bility of undesirable arm motions induced by the inertial forces and moments. 

In what follows, we shall present a control strategy for each phase. For simplicity, only the case with 
planar motion will be considered here. 

4.1 Maneuvering Strategy for Alignment and Acquisition Phases ■ • 

Let 3 =' (0 9 ) and ~ (0,r^ Q )C IR 2 denote the spatial domain of the OMV robot corresponding to a 

given set of angles ” and a specified r (position of the centroid of the CMV base relative to the inertial 

frame). Let T(r )~=1 i " { r :‘,r~ C f which corresponds to the set of all points in K swept out by the 

*■ * * —co ^f v til , R —co 

— aa 
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robot body as 0 varies through its admissible values specified by the set : ad while keeping r^ o stationary. 

To simplify the ensuing development, we assume that the object Z^ is stationary with respect to the inertial 
frame. If the GMV robot and the object are sufficiently close to each other initially such that Hr^) 0 
then the objective is to move the robot to a new position specified by r* at which I (r^ Q ) H * 0 and 
inf {|jx - x'i| j x € Hr ), x 11 € £ } > c > 0, where e is a given nonnegative number. To avoid a complex col¬ 

lision-free maneuvering problem*^ we restrict the maneuvers to straight-line translations without robot-body rota¬ 
tions and with locked joint angles . Now, we give a simple collision-free maneuvering strategy by representing 
the robot arms Z and E by line segments and the base Z by a closed rectangle as shown in Fig.3. Moreover, 
the object E Q iaassumed A to be a compact convex set with interior points (If Z° is not convex, we consider coO.^} 

instead of J ). We also impose the following geometric constraints! 
o 

(Cl) Link-length Constraints i 2, > £ , and *■, + *■< where w is the distance between the rotation axes 

of jointe 1 and 1*. The latter condition implies that a collision between links 1 and 1* is impossible. 

(C2) Joint-angle Constraints; Let € be a specified small positive angle and 


1 <W' ®i * 2tan "'(r^)' V M 2tan " 1 (i^7r)* 

I 

s - («, * l 2 +h)/2, s’ - («- 1 , + H 2 ,+ h)/2, Y * [(s - (s - ij) (s - h)/s] 2 , Y* ■ [ (s* - > (s' - ) (s' - h)/s 


0 X - cos (^/^J, ® 1 , - cos 

where 


We require: 


8 -e $ 0 X < tt + 0 i + e, -0 lf + c < 0 1 ,< w - 8 lt - e, 


(1) 

i 

' I 1 . 
( 2 ) 

(3) 


and 

[G.j <ir - F, i « 2,2*. 


(4) 


Condition (3) along with constraint (Cl) imply that a collision between arm 1 or arm 2 with the base is impossi¬ 
ble. Condition (4) avoids the possibility of link 2 (link 2’) folding back onto link 1 (link 1'). 

Let 0° and r° denote the initial set of angles (9°,6. ,0,, ,9 2 « t * le P° 3 *- t * ,on t * le ^ ase “ centro ^ re_ 

spectively. Evidently, co(Z (0 ,r° )) is a closed convex polygon. Q The straight-line collision—free maneuver- 
inq problem can be stated as ^ollowS? Given Z (0 # r^ o ) such that ^ find a direction vector ri 

such that Z (0°,r° + in) H Z » 0 for all real numbers a>0. 

R — —co — o 

A solution to this problem is given by the following maneuvering strategy: 

Case 1: If I and co(Z (9°,r 0 )) have no common interior points, then we move the robot along a straight-line 

r ■ g p r g Q 

path in the direction ir^ until 


inf { ]|x - x' 


: xerir* >, x* e z }=* e , 

— -co — o A 


(5) 
,,o o 




where n is the normal (directed toward co(Z f G ,r )) of any line separating the convex sets and co(Z^(0 <£^, 0 

and r* is the new position of the base-centroid Tsee Fig.4a). 

CO o o 

Case 2: If Z and co(Z O ,r° )) have common interior points, but Z <£co(Z (0 ,r )), then there exists an 

- o R — —CO ° * —co 

"exit edge" E of the polygon co<Z (0°,r° )) (i.e E is an edge which does not correspond to any link or edge of 
the base) such that E Pi Z t 0. R ~ C ° Let L(E) denote the line containing E, and iPy the projection operator 
from IR 2 onto L(E) in the°direction v. The maneuvering strategy for this case is to move the robot along a 

straight-line path in the direction -v until condition (5) is satisfied, where y is any direction such that 


f~(I flco(L (0 ,r ))Cint(E), 

VO R — co 


( 6 ) 


where int(E) denotes the interior of E(see Fig.4b). In general, there may exist a cone of directions y which 
satisfy (6). 

Case 3: Suppose that Z Cco(Z ( )°,r° )). Let E be the exit edge of co(Z 0°,£° )) associated with i. (i.e. 

—- * * o R — —CO o O K ^ 

the exit edge associated with the closed domain DCco(Z (c) , r^_ ^) ) containing whose boundary -3D is composed of 

E and one or more links and base-edges) (see Fig.4c). R Suppose there exists a°direction y such that 

T~(Z ; C int (£) ( - 1) 

y o 

arid 


co IIJJ-(£ )) n Z ( J°,r° ) = 0 
o y o R — —co 

are satisfied. Then we move the robot along a straight-line path in the direction -v until condition (5) is 
satisfied. Here, condition (8) implies that Z can be projected into L(E) in the direction y without any ob¬ 
structions from any part of the robot. 

Case 4: Suppose that Z C co( Z (0°,r° )) and there does not exist a direction y such that (7) and (8) are satis¬ 
fied simultaneously (see'Vig. 4dF. _CO Then it is impossible to achieve a collision-free straight-line maneu¬ 
ver for the given set of angles 0° = (0°,G°,0^,,,9^). In this case, it is necessary to change 0 until both 
conditions (7) and (8) are satisfied for some direction y. We propose to accomplish this by altering the joint 
anales only without introducing a base rotation. The joint angles should be adjusted such that the sequence of 
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domains {D ,'j-l,2,... } (defined in Case 3) generated by a sequence of joint-angle settings satisfy D.CD.^, 

i- 1,2 . 1 , 

„ < ' ►k. mhnf sufficiently far away from the object, the next step is to perform a rotational maneuver 

Having m wlth the object’s gripping direction n while holding all the joint angles at their 

to align the tHV fading wi ™ 3 followe d by a translationaI°mareuver along a line parallel to the line ccn- 

initial values (see Fig.SaK Thxs is^ollow pQ . nt Qn the robot bage is aliqned with a corresponding 

taining the gripping poin Eg 4 j 5 , Finally the joint angles are adjusted so that the object can be 

..Uht-lin.'U.n.i.tioh.l o, ». « W 1 U -14i* «* - 

th. 0,7 b „. stationary Csaa Fiq.5d). Thl. o<r,pl«l:e. th. aliqnm.nt phann. 

in th. acquisition pha... th. control .».«» quid., th. c«v .Ion, a should^ 

th. obj.ct uhil. ke.pijq all th. Joant with t a, ... . Sine. th. .l.n.ntary 

ZZTrl'll iSS t'^.U^t ^d .tuition pha... r.quir. controllin, at post tio v.ri.bl.. at a ti«. th. 
control problem is greatly simplified. This aspect will be discussed m the following section. 

4.2 Control Laws for Elementary Maneuvers 

motion for the CMy robot. Here, . ,, „ „ , r _„ «. is a onroach is adopted here instead of 

44 .rt.x^ 4 ^. — - - -- - 

base. . , 

bet the links of arm 1 (resp. arm 2 ) be labelled by 1 and 2 (resp 1 * ^^fo'each link “r. 

link 0 (see Fig.l). Adopting the notations of Asada and Slotine (3), the equations ot 

given by 


Link Is 


Link 2 


Link 1' 


f - f „ - ro,v , * 0, 

—0,1 —1. , 2 1—c 1 ~~ 

M a _ Xf — r X f I UJ, ~ * ( I m ll)- ) 3 0? 

4,1 " -1,2 4,cl 4,2 4,cl 4,1 1-1 -1 1-1 - 


—1,2 ‘ m 2—c2 * 2.' 

—1 1 2 “ —1 ,c2 *—1,2 ” *2 —2 ” —2 * ( ^2 —2^ “ —* 

4,1’ —1' ,2* --i.5cl.-a' 



5*0,1* 

- *v. 2 > y 4- 

X f 

,c 1 ’ —1 ’ ,2 ’ 

Link 

2 * : 

- n v = 0, 



— 1* ,2 




4. : 

- r X f 

• 4’,c 2* -l 

i ii “ 1 a • d*- 
, - * 

Link 

0 (QMV Base) 

: 



C 

- f - m v 

+ l, = 0, 


-1,0 

-0,1 o -40 



-1,0 

- 5*0,j. + 4’,co 

x f - r 

-F), 1 1 4, 


)* ,ci' * —0,1' - I 1’^ 1 ’'^1 1 ‘ <I 1 > -1 ,: 


at . x f -r V f -I , J-W X (I^)'*’5L S 2.' 

-1 0 “ 5*0,1’ 4‘,co —0,1’ 4,co -1,0 0-0 -0 0-3 -c 

a 1 cort£ rol force acting at the centroid of unk 0. 

For the case of a planar C*V robot, all the joint axes are along the z or a axis Let ^ and J denote ^tne 

*S2,*sn£.T r:» 


: 1^1 " *2-4 + 4-0,Cl" 4:1 " 4 ( £l.ci ’ 4,cl " £ 1 ,c2 ) 4:2 


4 V 44,c2 * *4=2 = 4 4 ' 

I,, L, , ♦ 1 „ * n , V d’ * 1,1- ‘ *2’ ( 4’ ,cl ~ 4a’,cl’ ' -l’,c2’' < 4:1’ ~ 1 1 ’ 4*' 


4’ 4* ,c2* 


1 »• 4 - .4 *• 4. 2^1. 

o-o . -1 2-2 *• i 


E„, 1,, - m (r 
2 * —2 * L —o f c o 


Io,cl )X 4l " m 2 —! ,cl ’ 4,cl ’ 4, c 2 * 4,co ! 


n>.. (r^, _ Q - E 0 . fC x* ) " 4r “ 4’ *4’ ,cl ' 4* ,cl* " -V ,c2’ * 4.’ ,co ! ^ 4:2’ 
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m.v , * m.v . * m,.v ,, + m„,v ♦ a v * f , €19) 

l-cl 2-c2 l'-cl' 2 c2 1 Cr-CO -c 

where Ub - w e . Equations (14)-(19) constitute a complete description of the motion of the planar GUV dual— airm 
robot. 

To obtain explicit forms for (14)-(19), we choose the body coordinate system with basis 0^ ■ (e^(t),e (t) # 
e (t) } and with the origin O on the axis of rotary Joint 1. Moreover, e (t) is directed from point 0 to~^o*. 
Tnla choice of origin 0 is preferred over that at the base-centroid, since the latter is usally not precisely 
Known. Now, we introduce the joint angles 0 , i*l,2,l|,2', and the OMV base attitude angle 0 as shown in Fig. 

3. Thus, the angular velocities u>. are related to the 9’a by ° 

i 1 

W l- S o^l' U 3 "VVV VVV' ‘VV'WV* <20) 


Note that all the angles 0^, with the exception of 0 q , are bo<iy referenced. Thus, they can be measured by nwemns 
of body-reference sensors. The base attitude angle f) must lie measured by means of an inertial-roference sensor. 

Since the CHV robot is autonomous, it is natural to use the hody coordinate system as the basic reference 

frame for the arm motions and for observations of the environment from the OMV robot. Expressing the position 


vectors r , with respect to the baais 8 , wo have 
i ,c j t 

r <* t ,{(csl)e (t) + (snl)e (t)l, r 

~o,c 1 cl -x —y —< 


• ii * f .,l(csl*)e (t) t (snl’)e (t)|, 
«' ,cl ' cl’ -x -y 


r -x e(t)*y e < t) , 

-o,co o,co—x 'o,co-y 

r , - (x - w)e (t.) + y e (t) , 

—o ,co o,co —x 'o,co— y ’ 


r , * w e (t) , 
—o .o' —x 


r , m (x -w)e (t.) + y e (t) , r, , ’ l ,f(osl2)e (t) + (snl2)e (t)l, 121) 

-o’,co o,co -x o,co—y —l ,c2 >:2 —x —y 

r. , - l ( (cal’2’)e <t) ♦ (snl’2’)e (t) I, r, , - (l ,-.’)( (csl)e <t) + (snl)e (t) }, 

—I’,c2’ c2 ’ —x —y — l,cl cl 1 -x -y 

r , - (fc - *,,)( (csl’)e (t) *■ (snl’)e (t)}, 

—1 ’,c1 cl’ 1 * —x —/ 

where sni »■ sin 9^, csi » cos 0 . , sn i j = sin <9 , * 0 .) and cm i ) =* cos <9 . ♦ 9 .). In writing down (21), we have a»ii»s~ 

ed that the centroid (marked "g" in Fig.l) l of Any link t of arm t or^2 is located inside link i along the line 

segment connecting the joints i and i+1. The centroid of link 0 (OMV base) specified by r has the time- 

invariant representation (x ,y ,0) with respect to the time dependent basis 8 • ~°* c ° 

o,co"o,co 1 ' t 

Let r (t) » x (t)e (t) + y (t)e (t) denoto the vector directed from the origin 0° of the inertial frame to thu 

origin 0 of the Body coordinate system C. Thus, 

B 

r * (x -y 9 - 2& y -x M*)e (t.) ^ (y *■ x d * 2x 9 - y 0 J )e (t). ?22) 

-o 0 * 000000 —x 'ooo 00 00 -y 

The accelerations of the link-centroidn v , i’O,l,2 , l’ ,2’ can he obtained by differentiating (21) (see Appendix 
for their explicit expressions). Substituting the explicit expressions for v and V leads to the following 
equations of motion for the (JMV robot; 

H(c^) t = u, 2 1) 


where ^ » (0^,0^ 


fl 2 , ' 0 o’ 

x »Y 

0 0 

) T , u * 

( 1 ^, 

,U 7 

) ' r ■) 

V » (v ,... .v 7 )i 
ments, and II(<j) 

is a 

is a 

vector-valued function 

7 “7 matrix of the form: 

of 0 and 'j 


' h ll 

h 12 

O' 

0 

h 15 

h lb 

h 17 


h 12 

h 22 

0 

0 


(i 

2f> 

h 

2 7 


0 

0 

h n 

h l4 


h - 

1*1 

h (7 

II ’ 

0 

0 

h 14 

h 44 

h 45 

h 

4U 

h 47 


h *l 

h w 


^,4 

V: 

h. 

' / 1 1 

h, , 

! / 


h 16 

h 2f, 

h 16 

h 4f, 

V, 

h 

»vf» 

n 


, h L7 

' h 27 

h l7 

h 47 

h 7 r > 

i) 

h 77 

The explicit expressions for the 

elements of 

il awl 

t tie 


Now, we consider the problem of deriving suttable control laws for the elementary maneuvers using math** — 
maticai model described by (22). Since these problems (or all the elementary maneuvers are intrinsically ide —— 
tical, we shall discuss only a specific case to illustrate the basic: ideas. 

Consider the problem of deriving a control law for aligning the uMV (leading n with a specified grijfing di¬ 
rection H while keeping all the joint angles and base-centroid position stationary. In this c.ise, we reijiti re 
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( 25 ) 


£<o - (o,o # o,o,b‘ o (t> ,o,o) T - Jet), 

whore 7 ,7 ,0. , i-l,2,l\2‘ are specified constants. Thus, equation (23) reduces to 

° ° 1 .. A (25) 

h 55 ( a )U o “ * V 

... (27) 

h i5 ia )0 o * + u i , i y 5. 

d ^ w , „ „ n atlfl AO V 0 d - 0 . Equation (26) can be rewritten as: 

Let 0 denote the base anqle such that » h R , and v q h 

° .. . (28) 

Ss®" 1 ’. " Vi® - V 

A simple control law for (2H) is qiveri by 

(23) 

t c - v rj <o:,2> ♦ h 05 ‘a><V 0 o f V‘V' 

(30) 

*, ■ vs® * \5® v-°»' V-v- 1 J ' 

tk. ,„oi„ (»> ana <»> a«p«,,a « 

shown that by choosing K and K properly, -uch con ro anqles and the base-centroid position remain 

perturbations (see [)!, Chapter r G). To ensure that all he )Otnt control law depending on the 

at their specified values during the heading alignmen ma f their specified values may be used, 

instantaneous deviations of the joint angles and base-centroid position from their P 

5. CONCLUDING REMARKS 

The key idea in the proposed control strategy for ^^^^^'^fjegrees'orfreed^n. These elementary 

maneuvers into a sequence of elementary maneuvers invol g operations in a space environment in which 

maneuvers are simple to performed they are particularly ^.bl.^or , they are being 

safety is a major factor. Although the results pres experiment involving a planar dual-arm maneuveratle 

extended presently to the general three-dimensional case An ^ this time, 

robot which is levitated above ground by an air bearing is in the planning stag 
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APPENDIX 

. , „ • i, i - ( t ) e (t) = -0 e (t), the accelerations of V.e 

By direct computation using (21) and the relations: e^tt) - - V o-x 

link-centroids are given by 


V 

-CO 

- r 
—o 

* r = r 

—o,co —o 

0 

r-< 

i 

,co< 

* X 

■> O, 

CO o' 

•• t) 

—X 

* « X 

o 

,CO ( 

D 

^cl 

a r 
—o 

+ r = r 

—o,cl —o 

' 'cl 

• <9 

o 

* v: 

cs 1 1 

<t" +■ ■ 

o 

) sn 

I re 

(t] 

~c2 

•• 
a r 
-o 

.. ... 
t r . t r 
-o,l —1 , 

c2 

r “ 

—o 

! J. (0 

' * 01 

) sn 1 

f V 

9 ♦ 

o 

9 ; 
L 


4- 

U, !0 +0.) 

1 o 1 

csl - 

V* 

id,) 
Q i 

* snl 

*■ 'c2 

(9 t 
o 

.9 v 

‘ L 

\\ 

' '7 

Kv 


+ w e (t) + 
—x 

—o 1 ,c 

r 

r 

—o 

'■ W0 2 

o 

*■ 'cl' 

(9 

o 

v> 

‘C! 


+ 

»• 

1W0 t i , 

o cl' 

(9 - 
o 


) cs 1 1 

- 1 

c 

l* ( V V 

' snl 

* * i 


• ‘)e (t) , 


c2 o 1 2 


(A-2) 
) : csl2 ie (t) 

—i 

(A-3) 
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/ , ,-r + we (t) + r , ,, + f,, ,, - r - (wO 2 + l,, (0 ♦ 0,,) ? csi ‘ , (0 ♦ 0. >snl* + 9 ,, <3 ♦ 0 1 , ♦ 0,, )snl*2* 

-c2 f -o —x -o 1 ,!* —l , ,c2* -o o l'o 1* 1* o l c2' o l* 2 f 

♦ * +6,, +6^,) 2 csl»2‘)e (t) + {w0 - 4,,(6 +0 tl )snl* - * _,<0 + 0., + 6_, ) 2 snl'2 • + , (0 ♦0.,)csl* 

c2 • o 1* 2' -x o l* o 1' c2 o 1’ 2' 1 o i 

+ i„,(0 + 0,. ♦ (L, ) 2 csl'2' )e It), (Ar-S) 

c2 # o 1’ 2* -y 


where r <t) is given by <22) # 

“O 

The elements (g^) of H (<j) are given by 

h u - I x ♦ I 2 ♦ + m 2 a[ ♦ 2£ l l c2 cs2 ♦ l^) , 

h 12 “ h 21 " X 2 + n 2 l c2 ll c2 + V a2> ' 


(A— 7 ) 


h » I, ♦ I, ♦ m. U 2 + !• , <y snl + x csl) i+ m { ■’ 2 4- 2?., I _cs2 ♦ 3 2 i- x U.csl * >• cs!2) + y (f.,snl ♦ > ,sr»l2) 1, 

15 1 2 1 cl cl o o 2 1 1 c2 c2 o 1 c2 o 1 c2 

(A-a) 

h., a h £ , ■ -m,2. .snl - m (snl + 5. snl2), h * h , <= m > ,csl + m.<?.,csl + 5. csl2) , (A-9) 

16 bl 1 cl 21 c2 1/ 71 1 cl 21 c2 

h 22 “ I 2* m 2^c2' h 25 mI 2 +m 2 l c2 a c2 * V 32 +X o Csl2 +y o Snl2) ' V ' “V^ 3 " 12 * *62' *27 " Vc2 Csl2 ‘ *72' 


I 2^ m 2 l c2' h 25* X 2 +m 2 e c2 a c2 + V 


(A-10) 

h 33 Wl l‘ +r 2’ +m l« i ci ( + m 2' a l' * n V , c2' Cs2 ' +t c2' ) ' *34^*43 * V * m 2* V•< ? c2• * *1- Cs2 ’> ' (A - U) 

h 35 ” X l* * X 2* * m l’ {f 'cl' + ? cl* <y o t:n1 ' * ( V w)C3l,) >* m 2 < U l* * 2 ) V 9 c2' C * 2 ' * ; 'c2' * (x o + w) < P ' l . csl * *- ? c 2 ,Csl *' 

+ y ((snl 1 + l ,snl*2*)}, (A-1J 

h 36 “ h 63 ■" m l ,i cl ,sn1 ' -m 2 t (? i' sn1 ' + * c2 . snl’2*) , h J? - h ?3 = , cl ,c81 ’ f ™ 2 . < V CSl ' ♦ * c2 ,csl ’2'), (A-i: 

h 44 aI 2* - m 2' i c2 1 ' h 4 5 “ X 2‘ +m 2* e c2' U c2* + V 92 ’ + (X o + w)C31 ’ 2 ' +y o Snl * 2,t ' <A-1 ' 

h 46 * *64 *“ m 2'^ c 2 ,Sn1 ' 2 '' h 47 " h 74 “ m 2* Z c2' Cs1,2 ’' <A " 1! 

h 5l* V X 2 + m l i cl (1 cl' X o,co Csl - y o,co SnU +m 2 {ll l + 2J '1 J c 2 CS2 * *c2 “ “o.co 11 !™ 1 * ? c2 C3l2) 


- y (l snl + l snl2) }, 
o,co 1 c2 

h * I ♦ m l , (l + 3 cs2 - x 
52 2 2 c2 c2 1 o,cc 


csl2 - y un!2 ), 
3 o.co 


^ ^ t- X U ( U,^-U 

I l' + I 2 ,+m 1 ,l cl‘ a clV + (w - X o.co ,csl, - y o.co anl,Um 2' ,i i' f2? -l- i c2* Cs2 * f J c2^ (W - X o,co )(? l* CSl ' 

r l csl'2')-y (!. sn 1 ’ f 3. sn 1'2' ) t, ^ 

c2* o # co 1' c2' 


Q* o # co i c*: 

r 2- + m 2* J 'c2' U c2« + ^l^ 92 ’ + <W ' X o r 


h 54 =I 2’ + m 2 ‘ ^2 • { e c2' + e i ,CS 
h 55 =I T +m l (i cl + (x o' X o,co H cl 


)cs1’2* - y snl'2’ ), 

1 r\ rA 


) l csl + (y - y M snl-xx - y y } ■*■ m*Z. 2 ♦ 2l!!. cs2 + 3} 
cl o o,co cl o o,co o o.cc' 21 1 c2 c2 


+ (x -x ) {l csl + l csl2) + (y - y )(i,snl*-i 1 snl2)-xx - yy ^ 
o o.co 1 c2 o ‘o,co 1 c2 o o.co o o.co 

+-m,.(!. 2 „t(x t2w-x ..csl' + (y -y ) ?. , . snl 1 + (x ♦ w) (w *• x ) - y y ) 

1' cl* o o.co cl' 'o 'o.co cl 1 o o.co o o,co 

+ , [ i 2 +-272 ,cs2 * + (x * 2w - x ) ( > cs 1 ’ *■ i ,csl ’ 2 *) * (y - y ) ( 2,, snl ’ +2 ,,snl': 

2’ 1' 1* c2 c2* o o.co 1' c2 'o o,co 1' cj* 

* \ \ _ .. ,. * r i 


(x ♦ w) (w - x ) - y y ) # 
o o,co o o,co 

her- " (y ~ .snl) + m (y 
56 1 ,i o # co cl 2 o,co 


- ',^ 1 - -’. c2 snl2 ) *» l .ty o ; co - 2 cl .sni , > ♦» 2 .<y 0fC0 - 1 1 . anl, - t c2 . !501,2 


h = m. ( 1 , csl - x ) ♦ m. ( J., csl *• ■' cs 12 - x ) * m, , (w ♦ i^iicsl' * x Q co 
57 1 cl o,co 2 1 c2 o.co 1 - 1 o.co 


r . Q ) + m 2 < (w +*^icsl' + ^- c2 ' csl 1 2 


x o,co J * 

-m ,2 , snl - m, ( i, snl + 3. _snl2) - m, , 3 
lcl 21 c2 1' cl 


h 65 = ‘ m i';ci snl 
h 66 = h 77 = m T' 


■"l' : c l ,sn1, - m 2* a i' 3n1 ’ + i c2' Snl,2,) 'Vo,co ‘ Vo' 


where 


h_ r = m.l ,csl + m. (i.csl * J csl2) ♦ m. , (w * *1 ,,csl') <-m-,(w+i,,csl' ♦ l csl’2') im x + m x , 
75 1 cl 2 1 c2 1' cl' 2' 1 c2' o o.co T o 

I T = I 0 + I 1 + I 2 4 ’ I 1 * + I 2 «* m T = m 0 , ' m ! * m T.» and a11 the remaining Ik 's are zero. 


h. .*s are zero. 

1 'l 
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(A— 27) 
(A-28) 


The components of V<5,g) are given explicitly by i 

V x - - 1 i cl {Uy o 6 o + x o ^)snlM 2 * o 0 o -y o ^)c.l} ♦ » 2 Wj 0 > <h* nl ' l c2 anl2) ♦ Wi^-y^M^cl 

+ t c2 csl2) - (2(§ o + 0 1 )9 2 + 02)t 1 ^ <;2 fln2}, (A-25) 

V 2 “ a 2 i c2 U l (S o +S l )ifln2 + (2 y o § o + X o § o ,snl2+ (2i o®o' y o § i >CSl2} ' <A " 26> 

V 3 - » 1 ^ cl .{^ o 0 o+ (x o -w,0j)snl. + <2i 0 § 0 -y 0 ^ )c8l ' }+ V {(2y oV <x 0 + W) § o> (1 l' sn1 ' + l c2' nnl ' 2 ' ] 

♦ (2x o § o‘ y o § o )a i' C “ 1, + a c2‘ C8l ‘ 2,) - < ®2' +2< ®o' § l‘ )5 2* )8n2 ’ } ' <A " 27 ’ 

V 4 -»2* 1 c2* U l'^o + § l' )2 “ n2 ' + (2y o°o * ‘V' 0 ^ 8 ' 11 ' 2 ’ + (2i oV^ ,C,1 ' 2,) ' <A " 28 ’ 

V 5 " -»l {(x o,co- t cl C8l)<2i o 6 o- y o®o - t cl<VV l8nl) * (y o f co- l cl Snl)<2y o 6 o + X o § o^cl ( V®l )2c8U} 

'»2 { (X o!co - V 81 " *c2 C8l2) < 2x A : Co ‘ * A * V “ nl ’ *C2 <*o * 4 1 * V 8 " 12)+ ^.co" V" 1 ‘ ^c2 8nl2) * 
( 2 y o 0 o + x q 0* + l 1 (0 o ♦9 1 ) Z cb1 ♦ 4 c2 ^o + \ + § 2 ) J CSl2)} - » 1 ,«x OfC0 -w^ cl , c 9 l‘M2x 0 9 0 -y 0 e= 

- t cV 6 o +l v t»nV) * ty o#co - l cV * T ' l,) ,2 y o ®o * X 0^0 * +W0 o )} 

- m 2 ,{(x oco -w - t^cal* -t c2 ,csl*2-)(2i o 0 o -y o 0j - i& 0 + * v ? •"!' * * c2 . <0 o + V + 0 2 . > 2 * n 1 ' 2 *) 

+ <y -*i. sn1 ' • l c2- 3nl ' 2,n2y o®o + X o § o + ^l• ( ^ 0 + ^l’ >2 cs ^* * ^c2' ^o +6 l' + ^2' ^ ca 2' - wf5 o> h 

0,00 1 ( fir- 29 ) 

V 6 “ 'V^o * X o*o> - ( Vcl * VlA + V*« l ’ -aWVW” 12 ~ ( V‘cl’ +m 2‘ l l- )( V 4 l* ,2cSr 

* V^A + - <V * vA " B o X o,co ®o’ (A ' 30) 

V 7 “ M tA X o ‘ y o^o ) ‘ Vcl + « 2 t l )( ®o + ®l )29nl "' n 2 ll c 2 t ®o +0 l 4 '^ 2 )23n12 ' (m l' l cl' * m 2' H* 1 ( ®o * V * 8nl ' 

m 2* ^c2' ( ^o + ^1' + ^2‘ >2 snl ' 2 ' - B o y o,co^o' (A ‘ 31) 

Note that H<q) is not the manipulator mass-inertia matrix. Its nonsymmetry ( i.e. »» Sl <a> - h i5 .M 5 , i-1, . 

7) is partially due tTThe fact that the control force f and the acceleration of the base-centroid are ex¬ 

pressed xn terms of the body coordinate system C with basis 8 The moment c ^ also , 

nonsymmetry of H( a ). It can be shown that the “manipulator nSss-inertia matTY* K ( a ) is related to H( a ) 

by 


(A-29) 


K ( a ) = 


(A- 32 / 


where I is the 4*4 identity matrix, and 
4 



0 

0 


csO 

snO 

(A-3 3) 

■snO 

csO 
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